Skip to content

fix: read fresh status before motion and gate the grasp handover - #86

Merged
cao-xiao-hao merged 2 commits into
nexform-tech:mainfrom
thetooler:fix/sdk-stale-state
Nov 19, 2026
Merged

cao-xiao-hao merged 2 commits into
nexform-tech:mainfrom
thetooler:fix/sdk-stale-state

Conversation

@thetooler

@thetooler thetooler commented Nov 18, 2026 •

Copy link
Copy Markdown
Contributor

Summary

Source-only fix for the stale-state and grasp-handover paths found on the
bench on 2026-11-18. This supersedes #85.

Changes

  • MotorController.poll() drains every queued status frame, bounded at
    MAX_DRAIN_FRAMES, and lets the newest one define the state.
    poll_until() accepts the update based on the target's rx_count, so
    another registered motor's frame cannot hide it.
  • GripperActions._fresh_state() requests a status frame (0xCC), retries,
    and refuses to plan when no frame answers; the final reach check uses the
    same path instead of a cache read.
  • The force-approach handover also requires the window average speed to be
    below MotionConfig.stall_speed_mm_s (2 mm/s).
  • A mid-hold CommError is converted to GraspResult(ok=False) after a
    zero-torque release, so the last hold torque is not left on the bus.
  • stall_speed_mm_s is validated as a non-negative finite number; the poll
    drain is capped.
  • Tests cover every behaviour change: the drained queue and newest-wins
    rule, the bounded drain, multi-motor poll_until, the slow creep that
    must not be handed over, an idle bus that must be refreshed, the
    stale-cache refusal, the final reach refresh, a mid-hold CommError, and
    the new validation.
  • Both README grasp sections document the true-stop gate and the slow-creep
    refusal path.

Why

On the bench, a grasp(5 N, hold_s=2) left the last hold torque on the bus.
The status cache still held the position from the start of the hold (78 mm)
while the jaws were at 26 mm; a following open() scheduled its ramp from
the cache, commanded ~0.7 rad of error on the first frame, and the jaws left
at 23 rad/s out of a 20 mm/s move.

Separately, a force-budgeted approach that crawls (about 8 mm/s against a
50 mm/s schedule) was classified as stalled by the 20 % window criterion and
handed to the torque-only hold while still in free travel; the hold then
accelerated the jaws across the stroke.

Behaviour

  • Status reads are current: the drain runs non-blocking after the first
    frame; timeout_s only applies to the wait for that first frame.
  • A motion never starts from a cached reading: if the motor does not answer
    the 0xCC request, the call raises CommError instead of guessing.
  • The final reached / ok decision also requires an answered frame.
  • open / close / goto / release and the ordinary 20 % stall criterion
    are otherwise unchanged.
  • MotionConfig.stall_speed_mm_s is new (default 2 mm/s); it is a policy
    threshold, not a measured hardware constant.

Testing

  • python3 -m unittest discover -s tests -t tests: 511 tests pass.
  • npx --yes markdownlint-cli2@0.23.3 README.md README.zh-CN.md: 0 issues.
  • Bench: the read-only smoke test passes; motion tests are being re-run on
    the updated branch.

Known limitation

A free creep below 2 mm/s can still be mistaken for a stop. It is
reproducible at 2.5 N: the approach creeps at about 1.9 mm/s and the hold
takes over before the closed stop. The threshold stays at 2 mm/s because
2.5 N is below the documented 3 N setpoint (the jaws may not close below
about 2.2 N and are marginal between 2.2 N and 3 N), so this is an
out-of-policy operating point; within the supported range the gate keeps at
least a 2x margin (a 5 N approach crawls at about 8 mm/s).

Issues

Refs #85 (superseded).

@cao-xiao-hao cao-xiao-hao left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Verdict: request changes

The diagnosis is right, and draining the status queue is the correct root-cause fix for the stale-cache path. Two things block merge: the behavioural changes have no tests, and the commit/PR metadata violate the repository rules. The technical points below are worth resolving before this lands.

Blocking

  1. No tests for either behaviour change.

    • MotorController.poll() no longer consumes one frame per call. Only a queued-frames test proves that the newest state wins; the current tests/fake_can.py cannot queue frames.
    • MotionConfig.stall_speed_mm_s is a new safety gate. The false-stop case (free travel at ~8 mm/s against a 50 mm/s schedule) is not covered by any test.
      Every new behaviour and every bug fix must ship with a test. The 498 existing tests passing does not exercise either change.
  2. Commit message. The subject is 82 characters (limit 72), and the footer carries Co-Authored-By: Claude Code <noreply@anthropic.com>. Agent attribution is not allowed. The PR body's "Generated with Claude Code" line should go too.

  3. PR body has no Changes or Issues section. Add them, with Refs #… / Closes #… if there is an issue.

  4. CI has not run. Run 44261468705 is action_required, so there is no test gate on this PR. It needs a run before merge.

Technical

  1. poll_until() can miss an update with more than one motor. poll() now returns only the newest motor whose frame was drained, and poll_until(motor) accepts the result only when it is motor. If another registered motor's frame is drained last, the target's update is invisible and the call times out even though motor.rx_count advanced. Reproduced with two registered motors and a queue [A, B]:

    poll_until(A, timeout_s=0.05) -> False | A.rx_count 0 -> 1
    

    The single-motor LiteGrip path is unaffected, but MotorController is public and supports multiple motors. Either expose every updated motor from poll(), or have poll_until() test motor.rx_count > prev_rx regardless of the returned motor.

  2. The known limitation sits on a normal operating point. At 2.5 N the free creep is ~1.9 mm/s, just below the new 2.0 mm/s gate, so the jaws can still be handed to the torque-only hold while still creeping. Reproducing at a normal grasp force means the threshold has no margin. Either include the stricter package (0.5 mm/s plus two confirm windows) or derive the gate from the force budget instead of a bench-tuned constant.

  3. The final reach check still reads the cache. actions.py:1852 still calls g.get_state() rather than _fresh_state(). get_state(wait=True) returns the cached reading after 50 ms when nobody answers, and the code does not inspect fresh, so reached / ok can be decided from a stale position — the class of bug this PR fixes.

  4. "Never plan from a stale cache" is weaker than the docstring says. When refresh_status() succeeds but no frame arrives, _fresh_state() accepts a cache up to the STALE_AFTER_S default (0.5 s) old. At the 20 mm/s open/close speed that is up to 10 mm of error. Consider a tighter threshold for motion planning, or require fresh.

  5. grasp() error contract changes. _hold_force() now calls _fresh_state(), which can raise CommError mid-hold. grasp() documents only CommandError, and its finally clears the release state without releasing torque. If callers are expected to handle an exception instead of HoldResult(ok=False), document it; otherwise convert it to a failed hold.

Nits

  • poll() drains until recv() returns None; on a bus that keeps delivering, the loop is bounded only by the bus. A maximum-drain cap would bound the added latency.
  • stall_speed_mm_s is not validated. A negative value makes moving always true, so the approach always refuses at the ceiling — safe, but silent.

Testing performed

python3 -m unittest discover -s tests -t tests on 840545a: 498 tests, OK.

The motor answers every control frame with a status frame.  Reading one
frame per call left the rest queued, and the age was stamped when a frame
was read, so an old position looked fresh.  poll() now drains the queue
(bounded) and lets the newest frame define the state.  poll_until()
checks the target's rx_count instead of the motor returned by the drain,
so another registered motor's frame cannot hide the update.

GripperActions._fresh_state() requests a status frame (0xCC) before every
motion, retries, and refuses to plan when no frame answers.  A cached
snapshot -- even one younger than STALE_AFTER_S -- is not enough: at
20 mm/s, half a second of silence is 10 mm of unknown.  The final reach
check now uses this path too, so reached/ok are not decided from the
pre-loop cache.

The force-approach handover also requires the window's average speed to
be below MotionConfig.stall_speed_mm_s (2 mm/s).  A force-budgeted
approach that crawls (~8 mm/s on this bench) is not contact; handing it
to the torque-only hold accelerates the jaws across the free stroke.
The threshold stays at 2 mm/s: the 2.5 N reproduction is below the
documented 3 N setpoint, where the grip is already marginal (README).

A mid-hold CommError is converted to GraspResult(ok=False) after a
zero-torque release, instead of leaving the last hold torque on the bus.
stall_speed_mm_s is validated, and the poll drain is capped.

Tests cover the drain and newest-wins behaviour, the bounded drain,
multi-motor poll_until, the slow creep that must not be handed over, an
idle bus that must be refreshed, the stale-cache refusal, the final reach
refresh, a mid-hold CommError, and the new validation.
The abort path now reads the cached position instead of calling
_fresh_state(). A zero-gain frame produces no force from q, so a fresh
position is not needed to stop the torque; requiring one delayed the
release by up to three 50 ms retries and, when the motor did not answer,
raised CommError without sending a single frame -- exactly when a stop is
needed most.
@cao-xiao-hao cao-xiao-hao changed the title fix: use the newest status frame and require a true stop before the grasp handover fix: read fresh status before motion and gate the grasp handover Nov 19, 2026
@cao-xiao-hao
cao-xiao-hao requested a review from qwtiger November 19, 2026 03:53
@cao-xiao-hao

Copy link
Copy Markdown
Contributor

Requested a review from qwtiger.

For context on the added commit: 03f5018 fixes the abort=True release path. It previously called _fresh_state() before sending the zero-torque frame, so when the motor did not answer the 0xCC request the call raised CommError and sent no frame. The abort path now uses the cached position (q is irrelevant at zero gain). A regression test covers it, and CI is green on 3.10 and 3.14.

@cao-xiao-hao

Copy link
Copy Markdown
Contributor

@qwtiger could you take this one? CI is green on 3.10 and 3.14, and the only remaining blocker is the stale changes-requested review. Commit 03f5018 is the e-stop release fix added after the original changes.

@cao-xiao-hao
cao-xiao-hao dismissed their stale review November 19, 2026 05:32

The requested changes were addressed in 67cefc7 and 03f5018; the e-stop regression is now covered by a test and CI is green. Dismissing the stale request.

@cao-xiao-hao
cao-xiao-hao merged commit b1d2cfe into nexform-tech:main Nov 19, 2026
1 check passed
@github-actions

Copy link
Copy Markdown

🎉 This PR is included in version 1.3.1 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

released Shipped in a release

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants